﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Engine
{
    /// <summary>
    /// Provides extended mathematical functionality
    /// </summary>
    public static class MathUtils
    {
        public static Random Random = new Random();


        public static void Swap<T>(ref T _a, ref T _b)
        {
            T buffer = _a;
            _a = _b;
            _b = buffer;
        }

        /// <summary>
        /// Converts a Vector2 to a point.
        /// </summary>
        public static Point Vector2ToPoint(Vector2 _vec)
        {
            return new Point((int)_vec.X, (int)_vec.Y);
        }

        public static void IntegrateVerlet(ref Vector2 _acceleration, ref Vector2 _velocity, ref Vector2 _position, float _timeDiff)
        {
            _position += _velocity * _timeDiff + (_acceleration / 2) * _timeDiff * _timeDiff;
            _velocity += _acceleration * _timeDiff;
        }

        public static void IntegrateVerlet(ref float _acceleration, ref float _velocity, ref float _position, float _timeDiff)
        {
            _position += _velocity * _timeDiff + (_acceleration / 2) * _timeDiff * _timeDiff;
            _velocity += _acceleration * _timeDiff;
        }

        public static void IntegrateRK4(ref Vector2 _acceleration, ref Vector2 _velocity, ref Vector2 _position, float _timeDiff)
        {
            Vector2 p2 = _position + 0.5f * _velocity * _timeDiff;
            Vector2 v2 = _velocity + 0.5f * _acceleration * _timeDiff;

            Vector2 p3 = _position + 0.5f * v2 * _timeDiff;
            Vector2 v3 = _velocity + 0.5f * _acceleration * _timeDiff;

            Vector2 p4 = _position + v3 * _timeDiff;
            Vector2 v4 = _velocity + _acceleration * _timeDiff;

            _position +=  (_timeDiff/6f)*(_velocity + 2*v2 + 2*v3 + v4);// _velocity * _timeDiff + (_acceleration / 2) * _timeDiff * _timeDiff;
            _velocity += _acceleration * _timeDiff;
        }

        /// <summary>
        /// 2D cross product defined as a1 * b2 - a2 * b2
        /// </summary>
        public static float Cross2D(Vector2 _vec1, Vector2 _vec2)
        {
            return _vec1.X * _vec2.Y - _vec1.Y * _vec2.X;
        }

        /// <summary>
        /// Returns the Vector [-a2,a1], orthogonal to _vec1
        /// </summary>
        public static Vector2 Cross2D(Vector2 _vec1)
        {
            return new Vector2(-_vec1.Y, _vec1.X);
        }

        public static Vector2 Project(Vector2 _a, Vector2 _b)
        {
            return Vector2.Dot(_a, _b) / _b.LengthSquared() * _b;
        }

        /// <summary>
        /// Returns the angle in radians of the rotation between _rotation and _normal
        /// </summary>
        public static float RotationForVector(Vector2 _rotation, Vector2 _normal)
        {
            if (_rotation == Vector2.Zero)
                return 0;
            _rotation.Normalize();
            float radians = (float)Math.Acos((double)Vector2.Dot(_normal, _rotation));
            if (_rotation.Y < 0)
                return radians;
            else
                return 2 * MathHelper.Pi - radians;
        }

        /// <summary>
        /// Returns the MTV to translate _b out of _a.
        /// </summary>
        public static Vector2 AABBOverlap(Rectangle _a, Rectangle _b)
        {
            int yOverlap = 0, xOverlap = 0;

            if (_a.Left < _b.Right && _a.Left > _b.Left)
                xOverlap = _b.Right - _a.Left;
            else if (_b.Left < _a.Right && _b.Left > _a.Left)
                xOverlap = -(_a.Right - _b.Left);

            if (_a.Top < _b.Bottom && _a.Top > _b.Top)
                yOverlap = _b.Bottom - _a.Top;
            else if (_b.Top < _a.Bottom && _b.Top > _a.Top)
                yOverlap = -(_a.Bottom - _b.Top);

            if (Math.Abs(yOverlap) < Math.Abs(xOverlap) || (xOverlap == 0 && yOverlap != 0))
                return new Vector2(0, yOverlap);
            else
                return new Vector2(xOverlap, 0);

        }
    }
}
